#include <cstdio>
#include <serial/serial.h>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <vector>
#include <stdint.h>
#include <sstream>

class Queue {
public:
    Queue(size_t capacity = 1024 * 4) : capacity(capacity), size(0), front(0), rear(0) {
        array.resize(capacity, 0);
    }
    bool isEmpty() const {
        return size == 0;
    }
    bool isFull() const {
        return size == capacity;
    }
    void enqueue(uint8_t element) {
        if (isFull()) {
            throw std::overflow_error("Queue is full");
        }
        array[rear] = element;
        size++;
        rear = (rear + 1) % capacity;
    }
    void dequeue() {
        if (isEmpty()) {
            throw std::underflow_error("Queue is empty");
        }
        size--;
        front = (front + 1) % capacity;
    }
    uint8_t getFront() const {
        return array[front];
    }
    size_t getQueueLength() const {
        return size;
    }
private:
    size_t capacity;
    size_t size;
    size_t front;
    size_t rear;
    std::vector<uint8_t> array;
};

class SerialNode : public rclcpp::Node {
public:
    SerialNode(const std::string &port, uint32_t baudrate)
        : Node("serial_node"), serialBuffer(4096) {
        // Initialize serial port
        try {
            ser.setPort(port);
            ser.setBaudrate(baudrate);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(to);
            ser.open();
        } catch (serial::IOException &e) {
            RCLCPP_ERROR(this->get_logger(), "Unable to open port");
        }
        if (ser.isOpen()) {
            RCLCPP_INFO(this->get_logger(), "Serial Port initialized");
        } else {
            RCLCPP_ERROR(this->get_logger(), "Serial Port not initialized");
        }

        // Define publisher and subscriber
        pub = this->create_publisher<std_msgs::msg::String>("serial_read", 10);
        sub = this->create_subscription<std_msgs::msg::String>(
            "serial_write", 10,
            std::bind(&SerialNode::writeSerial, this, std::placeholders::_1));
        
        // Create a timer to call the readSerial function periodically
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&SerialNode::readSerial, this));
    }

    void writeSerial(const std_msgs::msg::String::SharedPtr msg) {
        // Write to serial when a message is received
        if (ser.isOpen()) {
            ser.write(msg->data);
            RCLCPP_INFO(this->get_logger(), "Sent to serial: %s", msg->data.c_str());
        } else {
            RCLCPP_ERROR(this->get_logger(), "Serial port not open");
        }
    }

    // void readSerial() {
    //     // Read data from serial and enqueue it into the circular buffer
    //     if (ser.isOpen()) {
    //         std::vector<uint8_t> data = ser.read(ser.available());
    //         for (auto byte : data) {
    //             try {
    //                 serialBuffer.enqueue(byte);
    //             } catch (const std::overflow_error &e) {
    //                 RCLCPP_ERROR(this->get_logger(), "Queue is full, discarding data");
    //             }
    //         }
    //         processSerialData();
    //     } else {
    //         RCLCPP_ERROR(this->get_logger(), "Serial port not open");
    //     }
    // }
    void readSerial() {
        // Read data from serial and enqueue it into the circular buffer
        if (ser.isOpen()) {
            std::string data_str = ser.read(ser.available()); // Read data as string
            std::vector<uint8_t> data(data_str.begin(), data_str.end()); // Convert to vector<uint8_t>

            for (auto byte : data) {
                try {
                    serialBuffer.enqueue(byte);
                } catch (const std::overflow_error &e) {
                    RCLCPP_ERROR(this->get_logger(), "Queue is full, discarding data");
                }
            }
            processSerialData();
        } else {
            RCLCPP_ERROR(this->get_logger(), "Serial port not open");
        }
    }

private:
    void processSerialData() {
        // Check for data and publish messages based on buffer contents
        std_msgs::msg::String result;
        std::ostringstream ss;
        while (!serialBuffer.isEmpty()) {
            ss << static_cast<char>(serialBuffer.getFront());
            serialBuffer.dequeue();
        }
        result.data = ss.str();
        if (!result.data.empty()) {
            pub->publish(result);
            RCLCPP_INFO(this->get_logger(), "Received from serial: %s", result.data.c_str());
        }
    }

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub;
    rclcpp::TimerBase::SharedPtr timer_;
    serial::Serial ser;
    Queue serialBuffer;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SerialNode>("/dev/ttyUSB0", 115200);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

